﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading.Tasks;

namespace BoardController.Helper
{
    public abstract class Device
    {
        protected Device(int addrdev)
        {
            addrDev = addrdev;
        }

        public int addrDev;
        public abstract void Update(Msg msg);
        public abstract string[] Values();

        public int Bandth = 5;
    }

    public class VitualDevice : Device
    {
        public double YawBase;
        public double PitchBase;
        public double RollBase;

        public double YawCurrent;
        public double PitchCurrent;
        public double RollCurrent;

        public VitualDevice(int addrDev) : base(addrDev)
        {
        }
        public override void Update(Msg msg)
        {
            if (addrDev != msg.Device)
            {
                return;
            }
        }

        public override string[] Values()
        {
            List<string> temp = new List<string>();

            temp.Add(YawCurrent.ToString("f3"));
            temp.Add(PitchCurrent.ToString("f3"));
            temp.Add(RollCurrent.ToString("f3"));

            temp.Add(YawBase.ToString("f3"));
            temp.Add(PitchBase.ToString("f3"));
            temp.Add(RollBase.ToString("f3"));

            return temp.ToArray();
        }
    }

    public class IMU : Device
    {

        public IMUTime TIME { set; get; }

        public int IntTime { set; get; }

        // 0 停止 1 正转 2 反转
        public int PumpFlag { set; get; }

        private double XACC;
        private double YACC;
        private double ZACC;

        public DataFilter FilteredXACC = new DataFilter(5);
        public DataFilter FilteredYACC = new DataFilter(5);
        public DataFilter FilteredZACC = new DataFilter(5);

        private double ROTXANGLE;
        private double ROTYANGLE;
        private double ROTZANGLE;

        public DataFilter FilteredXAngle = new DataFilter(5);
        public DataFilter FilteredYAngle = new DataFilter(5);
        public DataFilter FilteredZAngle = new DataFilter(5);

        private double ROTXSPEED;
        private double ROTYSPEED;
        private double ROTZSPEED;

        public DataFilter FilteredXRotSpeed = new DataFilter(5);
        public DataFilter FilteredYRotSpeed = new DataFilter(5);
        public DataFilter FilteredZRotSpeed = new DataFilter(5);

        private double ROTXSPEED2;
        private double ROTYSPEED2;
        private double ROTZSPEED2;

        public DataFilter FilteredXRotSpeed2 = new DataFilter(5);
        public DataFilter FilteredYRotSpeed2 = new DataFilter(5);
        public DataFilter FilteredZRotSpeed2 = new DataFilter(5);


        private double LightX;
        private double LightY;


        public DataFilter FilteredXLight = new DataFilter(5);
        public DataFilter FilteredYLight = new DataFilter(5);

        public double Yaw;
        public double Pitch;
        public double Roll;

        public override void Update(Msg msg)
        {
            if (addrDev != msg.Device)
            {
                return;
            }

            IMUTime newTime = ComputeTime(msg.Data.ToArray());
            double diffTime = TIME.Difference(newTime);
            TIME = newTime;

            IntTime = msg.Data[2] * 256 + msg.Data[3];

            var xacc_raw = ComputeAcc(msg.Data[2 * 2], msg.Data[2 * 2 + 1]);
            var yacc_raw = ComputeAcc(msg.Data[3 * 2], msg.Data[3 * 2 + 1]);
            var zacc_raw = ComputeAcc(msg.Data[4 * 2], msg.Data[4 * 2 + 1]);

            ROTXSPEED = ComputeAngleSpeed(msg.Data[5 * 2], msg.Data[5 * 2 + 1]);
            ROTYSPEED = ComputeAngleSpeed(msg.Data[6 * 2], msg.Data[6 * 2 + 1]);
            ROTZSPEED = ComputeAngleSpeed(msg.Data[7 * 2], msg.Data[7 * 2 + 1]);

            ROTXSPEED2 = ComputeAngleSpeed(msg.Data[8 * 2], msg.Data[8 * 2 + 1]);
            ROTYSPEED2 = ComputeAngleSpeed(msg.Data[9 * 2], msg.Data[9 * 2 + 1]);
            ROTZSPEED2 = ComputeAngleSpeed(msg.Data[10 * 2], msg.Data[10 * 2 + 1]);

            var xRotAngle = ComputeAngle(msg.Data[11 * 2], msg.Data[11 * 2 + 1]);
            var yRotAngle = ComputeAngle(msg.Data[12 * 2], msg.Data[12 * 2 + 1]);
            var zRotAngle = ComputeAngle(msg.Data[13 * 2], msg.Data[13 * 2 + 1]);

            // 坐标变换, 由于IMU的安装方式不同，需进行坐标变换使得y轴正方向垂直墙面，x轴正方向向右，z轴垂直向上
            // 维特智能IMU逆时针旋转时角速度为正值
            //ROTXANGLE = (yRotAngle);
            //ROTYANGLE = -xRotAngle;
            //ROTZANGLE = zRotAngle;

            //XACC = yacc_raw;
            //YACC = -xacc_raw;
            //ZACC = zacc_raw;

            // 加速度去除重力分量影响

            //yacc_raw = yacc_raw - Math.Sin(xRotAngle) * Math.Cos(yRotAngle);
            //zacc_raw = zacc_raw - Math.Cos(xRotAngle) * Math.Cos(yRotAngle);
            //xacc_raw = xacc_raw + Math.Sin(yRotAngle);

            // Debug.Print(string.Format("{0} --- {1} ---- {2}", ROTXSPEED, ROTYSPEED, ROTZSPEED));
            //Debug.Print(string.Format("yacc: {0} --- zacc: {1} ----xacc:{2}", yacc_raw, zacc_raw,xacc_raw)); 

            ROTXANGLE = -yRotAngle;
            ROTYANGLE = xRotAngle;
            ROTZANGLE = zRotAngle;

            XACC = -yacc_raw;
            YACC = xacc_raw;
            ZACC = zacc_raw;

            LightX = ComputeDist(msg.Data[14 * 2], msg.Data[14 * 2 + 1]);
            LightY = ComputeDist(msg.Data[15 * 2], msg.Data[15 * 2 + 1]);
            PumpFlag = msg.Data[20 * 2 + 1];

            //Console.WriteLine("PumpFlag: " + PumpFlag);

            FilteredXACC.Insert(XACC);
            FilteredYACC.Insert(YACC);
            FilteredZACC.Insert(ZACC);

            FilteredXAngle.Insert(ROTXANGLE);
            FilteredYAngle.Insert(ROTYANGLE);
            FilteredZAngle.Insert(ROTZANGLE);

            FilteredXRotSpeed.Insert(ROTXSPEED);
            FilteredYRotSpeed.Insert(ROTYSPEED);
            FilteredZRotSpeed.Insert(ROTZSPEED);

            FilteredXRotSpeed2.Insert(ROTXSPEED2);
            FilteredYRotSpeed2.Insert(ROTYSPEED2);
            FilteredZRotSpeed2.Insert(ROTZSPEED2);

            FilteredXLight.Insert(LightX);
            FilteredYLight.Insert(LightY);

            Yaw = FilteredZAngle.Average();
            Pitch = FilteredXAngle.Average();
            Roll = FilteredYAngle.Average();
        }

        public double Decoy(double oldv, double newv)
        {
            double lamda = 0.5;
            return lamda * oldv + (1 - lamda) * newv;
        }
        private int ComputeDist(byte h, byte l)
        {
            short hh = (short)(h << 8);
            int x = hh | l;
            return x;
        }

        public override string[] Values()
        {
            List<string> temp = new List<string>();

            temp.Add(FilteredXACC.Average().ToString("f2"));
            temp.Add(FilteredYACC.Average().ToString("f2"));
            temp.Add(FilteredZACC.Average().ToString("f2"));

            temp.Add(FilteredXAngle.Average().ToString("f2"));
            temp.Add(FilteredYAngle.Average().ToString("f2"));
            temp.Add(FilteredZAngle.Average().ToString("f2"));

            temp.Add(FilteredXRotSpeed.Average().ToString("f2"));
            temp.Add(FilteredYRotSpeed.Average().ToString("f2"));
            temp.Add(FilteredZRotSpeed.Average().ToString("f2"));

            temp.Add(FilteredXRotSpeed2.Average().ToString("f2"));
            temp.Add(FilteredYRotSpeed2.Average().ToString("f2"));
            temp.Add(FilteredZRotSpeed2.Average().ToString("f2"));

            temp.Add(FilteredXLight.Average().ToString("f2"));
            temp.Add(FilteredYLight.Average().ToString("f2"));

            temp.Add(IntTime.ToString());
            return temp.ToArray();
        }

        public IMUTime ComputeTime(byte[] data)
        {
            IMUTime time = new IMUTime();
            time.Day = data[0];
            time.Hour = data[1];
            time.Minute = data[2];
            time.Second = data[3];
            time.Milli = data[4] * 256 + data[5];
            return time;
        }

        private double ComputeAcc(byte h, byte l)
        {
            short hh = (short)(h << 8);
            return (double)(hh + l) / 32768 * 16;
        }

        private double ComputeAngleSpeed(byte h, byte l)
        {
            short hh = (short)(h << 8);
            return (double)(hh + l) / 32768 * 2000;
        }

        private double ComputeAngle(byte h, byte l)
        {
            short hh = (short)(h << 8);
            return (double)(hh + l) / 32768 * 180;
        }

        private double ComputeMagnet(byte h, byte l)
        {
            short hh = (short)(h << 8);
            return (double)(hh + l);
        }

        public IMU(int addrdev) : base(addrdev)
        {
            TIME = new IMUTime();
        }
    }

    //class Laser : Device
    //{
    //    const int median = 20;
    //    const int mean = 20;
    //    private MedianList leftmedian = new MedianList(median);
    //    private MeanList leftmean = new MeanList(mean);

    //    private MedianList rightmedian = new MedianList(median);
    //    private MeanList rightmean = new MeanList(mean);

    //    public double LeftDistance
    //    {
    //        set
    //        {
    //            leftmedian.Push(value);
    //            var a = leftmedian.Pop();
    //            leftmean.Push(a);
    //        }

    //        get { return leftmean.Pop(); }
    //    }
    //    public double RightDistance
    //    {
    //        set
    //        {
    //            rightmedian.Push(value);
    //            var a = rightmedian.Pop();
    //            rightmean.Push(a);
    //        }

    //        get { return rightmean.Pop(); }
    //    }


    //    public double Mean { set; get; }


    //    public Laser(int addrdev) : base(addrdev)
    //    {
    //    }

    //    public override void Update(Msg msg)
    //    {
    //        if (addrDev != msg.Device)
    //        {
    //            return;
    //        }
    //    }

    //    public override string[] Values()
    //    {
    //        List<string> temp = new List<string>();

    //        temp.Add(LeftDistance.ToString());
    //        temp.Add(RightDistance.ToString());
    //        temp.Add(Mean.ToString());

    //        return temp.ToArray();
    //    }
    //}

    /// <summary>
    /// 标记集合: 用来存储稳台板上闭环控制的标记集合
    /// </summary>
    public class TagCollection : Device
    {
        public int pumpFlag { set; get; }

        public TagCollection(int addrdev) : base(addrdev)
        {
        }

        public override void Update(Msg msg)
        {
            if (addrDev != msg.Device)
            {
                return;
            }
            if (msg.Function == 0x10)
            {
                return;
            }
            pumpFlag = msg.Data[10 * 2] * 256 + msg.Data[10 * 2 + 1];
            if (1 == pumpFlag)
            {
                LogHelper.WriteLog("pump open now!");
            }
        }

        public override string[] Values()
        {
            List<string> temp = new List<string>();
            temp.Add(pumpFlag.ToString());
            return temp.ToArray();
        }
    }

    public class Board : Device
    {
        public double LeftHeight { set; get; }
        public double RightHeight { set; get; }

        public double LeftSpeed { set; get; }

        public double RightSpeed { set; get; }

        public int SlidePos { set; get; }

        public int SlideSpeed { set; get; }

        public int SlideFlag { set; get; }

        public int Emergence { set; get; }

        public int Printer { set; get; }

        public override void Update(Msg msg)
        {
            if (addrDev != msg.Device)
            {
                return;
            }

            if (msg.Function == 0x10)
            {
                return;
            }

            LeftHeight = msg.Data[0 * 2] * 256 + msg.Data[0 * 2 + 1] - 20000;
            LeftHeight = LeftHeight / 100;

            RightHeight = msg.Data[1 * 2] * 256 + msg.Data[1 * 2 + 1] - 20000;
            RightHeight = RightHeight / 100;

            LeftSpeed = 0;
            RightSpeed = 0;

            SlidePos = msg.Data[5 * 2] * 256 + msg.Data[5 * 2 + 1];

            SlideSpeed = msg.Data[7 * 2] * 256 + msg.Data[7 * 2 + 1];
            SlideFlag = msg.Data[8 * 2] * 256 + msg.Data[8 * 2 + 1];

            Emergence = msg.Data[9 * 2] * 256 + msg.Data[9 * 2 + 1];

            if (1 == Emergence)
            {
                LogHelper.WriteLog("open emergence switch!");

            }
            Printer = msg.Data[10 * 2] * 256 + msg.Data[10 * 2 + 1];
            if (1 == Printer)
            {
                LogHelper.WriteLog("combine open now!");
            }
        }

        public override string[] Values()
        {
            List<string> temp = new List<string>();

            temp.Add(LeftHeight.ToString());
            temp.Add(RightHeight.ToString());

            temp.Add(SlidePos.ToString());
            temp.Add(SlideFlag.ToString());

            temp.Add(Emergence.ToString());
            temp.Add(Printer.ToString());

            return temp.ToArray();
        }

        public Board(int addrdev) : base(addrdev)
        {
        }
    }
}
